from L1_laser.mj import RunMujoco
import time
from L1_laser.ros2_publisher import ROS2_Set
import config as cfg
import subprocess
import os
from threading import Thread


def run_rviz2():
    current_directory = os.path.dirname(os.path.abspath(__file__))
    command = ["rviz2", "-d", current_directory + "/rviz2/show_point_laser.rviz"]
    process = subprocess.Popen(
        command, 
        stdout=subprocess.PIPE, 
        stderr=subprocess.PIPE, 
        text=True
    )
    stdout, stderr = process.communicate()
    print(stdout)
    print(stderr)


if __name__ == "__main__":

    runMujoco = RunMujoco(cfg.L1_XML_PATH)
    runMujoco.run()
    
    if cfg.ENABLE_ROS2_PUBLISHER:
        node = ROS2_Set()
        node.launch_publisher()
        laser_sensor_data_get_interval = 1.0 / cfg.LASER_POINT_GET_FREQ
    if cfg.ENABLE_RVIZ2:
        rviz2_thread = Thread(target=run_rviz2)
        rviz2_thread.daemon = True
        rviz2_thread.start()
    
    while True:
        laser_sensor_data = runMujoco.get_laser_sensor_data()
        if cfg.ENABLE_ROS2_PUBLISHER:
            node.set_data(laser_sensor_data)
        time.sleep(laser_sensor_data_get_interval)